#include "headfile.h"
#include "math.h"

float w[SAMPLENUMBER];
float sin_tab[SAMPLENUMBER], cos_tab[SAMPLENUMBER];

void InitForFFT()
{
    int i;

    for (i = 0; i < SAMPLENUMBER; i++)
    {
        sin_tab[i] = sin(PI * 2 * i / SAMPLENUMBER);
        cos_tab[i] = cos(PI * 2 * i / SAMPLENUMBER);
    }
}

void FFT(float dataR[], float dataI[])
{
    int x0, x1, x2, x3, x4, x5, x6, x7, xx;
    int i, j, k, b, p, L;
    float TR, TI, temp;

    for (i = 0; i < SAMPLENUMBER; i++)
    {
        x0 = x1 = x2 = x3 = x4 = x5 = x6 = x7 = 0;
        x0 = i & 0x01;
        x1 = (i / 2) & 0x01;
        x2 = (i / 4) & 0x01;
        x3 = (i / 8) & 0x01;
        x4 = (i / 16) & 0x01;
        x5 = (i / 32) & 0x01;
        x6 = (i / 64) & 0x01;
        x7 = (i / 128) & 0x01;
        xx = x0 * 128 + x1 * 64 + x2 * 32 + x3 * 16 + x4 * 8 + x5 * 4 + x6 * 2 + x7;
        dataI[xx] = dataR[i];
    }
    for (i = 0; i < SAMPLENUMBER; i++)
    {
        dataR[i] = dataI[i];
        dataI[i] = 0;
    }

    /************** following code FFT *******************/
    for (L = 1; L <= 8; L++)
    { /* for(1) */
        b = 1;
        i = L - 1;
        while (i > 0)
        {
            b = b * 2;
            i--;
        }                            /* b= 2^(L-1) */
        for (j = 0; j <= b - 1; j++) /* for (2) */
        {
            p = 1;
            i = 8 - L;
            while (i > 0) /* p=pow(2,7-L)*j; */
            {
                p = p * 2;
                i--;
            }
            p = p * j;
            for (k = j; k < 256; k = k + 2 * b) /* for (3) */
            {
                TR = dataR[k];
                TI = dataI[k];
                temp = dataR[k + b];
                dataR[k] = dataR[k] + dataR[k + b] * cos_tab[p] + dataI[k + b] * sin_tab[p];
                dataI[k] = dataI[k] - dataR[k + b] * sin_tab[p] + dataI[k + b] * cos_tab[p];
                dataR[k + b] = TR - dataR[k + b] * cos_tab[p] - dataI[k + b] * sin_tab[p];
                dataI[k + b] = TI + temp * sin_tab[p] - dataI[k + b] * cos_tab[p];
            } /* END for (3) */
        }     /* END for (2) */
    }         /* END for (1) */
    for (i = 0; i < SAMPLENUMBER / 2; i++)
    {
        w[i] = sqrt(dataR[i] * dataR[i] + dataI[i] * dataI[i]);
    }
} /* END FFT */
